/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-07-17     MBW       the first version
 */
#include "sins.h"
_SINS sins = {0};
//竖直方向惯导融合
float sins_high(float aacz , float height )
{
    //融合周期检测
//    time_check(&sins_high_time);
    //高度状态差 ，气压计与历史惯导高度差值（cm）,采用历史惯导数据原因：气压观测数据的延时造成与惯导不一致，故采用历史惯导值进行作差
    sins.state_delta.z = height - sins.pos_history[high_delay].z;

                                                    //三路积分反馈量纠正惯导
    sins.acc_cor.z +=  sins.state_delta.z * K_ACC_ZER * 5 * 0.001f;
    sins.vel_cor.z +=  sins.state_delta.z * K_VEL_ZER * 5 * 0.001f;
    sins.pos_cor.z +=  sins.state_delta.z * K_POS_ZER * 5 * 0.001f;

    sins.last_acc.z = sins.acc.z ;                  //存储本次加速度作为上次加速度
    sins.acc.z = aacz + sins.acc_cor.z;       //惯导加速度(本次加速度)

    //v = a*t                                       //速度增量
    sins.vel_delta.z = (sins.last_acc.z + sins.acc.z)*0.5f*5*0.001f;

    //s = v0*t + a*t^2/2  =  v0*t + v*t/2
                                                    //原始位置
    sins.pos_org.z += (sins.vel.z + 0.5f*sins.vel_delta.z)*5*0.001f;
    sins.pos.z = sins.pos_org.z + sins.pos_cor.z;   //矫正后的位置

    sins.vel_org.z += sins.vel_delta.z;             //原始速度
    sins.vel.z = sins.vel_org.z + sins.vel_cor.z;   //矫正后的速度

                                                    //历史高度数据存储
    for(sins.pos_delay_cnt.z=pos_delay_num-1;sins.pos_delay_cnt.z>0;sins.pos_delay_cnt.z--)
    {
        sins.pos_history[sins.pos_delay_cnt.z].z = sins.pos_history[sins.pos_delay_cnt.z-1].z;
    }
    sins.pos_history[0].z = sins.pos.z;


}
